#include "filter.h"

 /**
  * @brief 一维卡尔曼滤波
  * 
  * @param EKF 卡尔曼滤波器系数结构体
  * @param input 当前系统的输入
  * @return float 滤波后的输出结果
  */
float KalmanFilter(KalmanFilter_t *EKF, float input)
{
    EKF->NewP = EKF->LastP + EKF->Q;                    // 预测
    EKF->Kg = EKF->NewP / (EKF->NewP + EKF->R);         // 更新卡尔曼增益
    EKF->Out = EKF->Out + EKF->Kg * (input - EKF->Out); // 最优估计
    EKF->LastP = (1 - EKF->Kg) * EKF->NewP;             // 更新协方差
    return EKF->Out;
}

/**
 * @brief 一阶互补滤波
 * 
 * @param x1 输入变量1
 * @param x2 输入变量2
 * @param x1_factor 对x1的影响因子
 * @return float 
 */
float Complementary_Filter(float x1, float x2, float x1_factor)
{
    float out = 0;

    out = x1_factor * x1 + (1 - x1_factor) * x2;
    return out;
}


